#include <buola/scene/transform/crevolutionjoint.h>

namespace buola { namespace scene {

CRevolutionJoint::CRevolutionJoint(const mat::CVec3d &pTranslation,const mat::CVec3d &pAxis,double pOffset,
                                   double pInitialValue,const std::string &pName)
    :   CTransform(pName)
    ,   mTranslation(pTranslation)
    ,   mAxis(pAxis)
    ,   mOffset(pOffset)
    ,   mValue(pInitialValue)
{
}

void CRevolutionJoint::SetTranslation(const mat::CVec3d &pTrans)
{
    mTranslation=pTrans;
    mLocalTransformValid=false;
}

void CRevolutionJoint::SetAxis(const mat::CVec3d &pAxis)
{
    mAxis=pAxis;
    mLocalTransformValid=false;
}

void CRevolutionJoint::SetOffset(double pOffset)
{
    mOffset=pOffset;
    mLocalTransformValid=false;
}

void CRevolutionJoint::SetValue(double pValue)
{
    mValue=pValue;
    mLocalTransformValid=false;
}

mat::CMat34d CRevolutionJoint::GetLocalTransformForValue(double pValue)
{
    return mat::rotation_translation_3d(mat::CQuat_d(pValue+mOffset,mAxis),mTranslation);
}

mat::CMat34d CRevolutionJoint::GetWorldTransformForValue(double pValue)
{
    if(Parent())
        return Parent()->GetWorldTransform()*GetLocalTransformForValue(pValue);
    else
        return GetLocalTransformForValue(pValue);
}

void CRevolutionJoint::CalcLocalTransform()
{
    mLocalTransform=mat::rotation_translation_3d(mat::CQuat_d(mValue+mOffset,mAxis),mTranslation);
    mLocalTransformValid=true;
}

PRevolutionJoint find_revolution_joint(const PTransform &pWhere,const std::string &pName)
{
    PRevolutionJoint lReturn=pWhere->FindDescendantOfType<CRevolutionJoint>(
                            [&pName](const PRevolutionJoint &pJ){return pJ->Name()==pName;});
    return lReturn;
}

/*namespace scene*/ } /*namespace buola*/ }
